/********************************************************************
* This code needs Blynk app to be installed into phone              *
*                                                                   *
* Used Blynk widgets are: joystick for controls and Horizontal      *
* value display                                                     *
*                                                                   *
*  CC-BY-SA Jari Laru, Fab Academy 2020                             *
 ********************************************************************/



/* ESP2866 and Blynk Libraries are included */
#define BLYNK_PRINT Serial /* This library adds blynk spesific info to serial communication between Arduino IDE and board, can be removed after testing */
// #include <Servo.h> Servo Library is disabled, because I did simplify implementation and removed servo controlled precision steering. 
#include <ESP8266WiFi.h> /* ESP wifi Library */
#include <BlynkSimpleEsp8266.h> /* Blynk Library */

/* Pins for ultrasonic sensors are defined here */
#define TRIGGERPIN 1
#define ECHOPIN  3

/*  L293D motor enable pins are defined here */
int rightMotorEN = 14 ; /* GPIO14 -> Motor-A Enable  */
int leftMotorEN =  4 ;  /* GPIO12 -> Motor-B Enable */

/* L293D motor control pins are defined here */
int leftMotorForward = 13;     /* GPIO5 -> IN3   */
int rightMotorForward = 16;   /* GPIO4 -> IN1  */
int leftMotorBackward = 5;    /* GPIO10  -> IN4   */
int rightMotorBackward = 12;  /* GPIO9 -> IN2  */


/* Auth token for Blynk */
char auth[] = "pw-muO8rOeGG-6P-JceBYPKqIsDskb-q";

/* Local wifi network settings for Blynk. HOX! If you are running own server, then server and this client needs be in same SSID */
char ssid[] = "ubinotko1";
char pass[] = "";

/* Local Blynk Server adress. It can be run also by using cloud service, but then amount free UX components is limited. 
 *  Own server has unlimited access.  In my case Rasperry Pi is server */

char server[] = "192.168.100.10";




// Reading ultrasonic sensor and sending readings to Blynk horizontal bar

BLYNK_READ(9) {

  long duration, distance;
  digitalWrite(TRIGGERPIN, LOW);  
  delayMicroseconds(3); 
  
  digitalWrite(TRIGGERPIN, HIGH);
  delayMicroseconds(12); 
  
  digitalWrite(TRIGGERPIN, LOW);
  duration = pulseIn(ECHOPIN, HIGH);
  distance = (duration/2) / 29.1;
  Serial.print(distance);
  Serial.println("Cm");

  Blynk.virtualWrite(9,distance);
}

// Handling Joystick data from Blynk and controlling left and right motors based on that
BLYNK_WRITE(V3){ 
int x = param[0].asInt(); 
int y = param[1].asInt();  

 // Do something with x and y
  Serial.print("X = ");
  Serial.print(x);
  Serial.print("; Y = ");
  Serial.println(y);

 if(y == 100)
 GoForward();  //Forward 
 
 else if(y == -100) 
 GoBackward(); //Backward  
 
 else if(x > 100) 
 TurnRight(); // Right turn

 else if(x < -100)  // Left turn
TurnLeft();
 
 else if (x == 0 && y == 0)
 Stop();
} 


// GO FORWARD //
void  GoForward(void)   
{
  Serial.println();
  Serial.print("Go to forward");
  analogWrite(leftMotorEN,255);
  analogWrite(rightMotorEN,255);
  digitalWrite(leftMotorForward,HIGH);
  digitalWrite(rightMotorForward,HIGH);
  digitalWrite(leftMotorBackward,LOW);
  digitalWrite(rightMotorBackward,LOW);
}

// Go backward //
void GoBackward(void)   
{
  Serial.println();
  Serial.print("Go to backward");
  analogWrite(leftMotorEN,255);
  analogWrite(rightMotorEN,255);
  digitalWrite(leftMotorBackward,HIGH);
  digitalWrite(rightMotorBackward,HIGH);
  digitalWrite(leftMotorForward,LOW);
  digitalWrite(rightMotorForward,LOW);
}

// TURN LEFT //
void TurnLeft(void)   
{
   Serial.println();
   Serial.print("turn to left");
  analogWrite(leftMotorEN,255);
  analogWrite(rightMotorEN,255); 
  digitalWrite(leftMotorForward,LOW);
  digitalWrite(rightMotorForward,HIGH);
  digitalWrite(rightMotorBackward,LOW);
  digitalWrite(leftMotorBackward,HIGH);  
}

// TURN RIGHT //
void TurnRight(void)   
{
  Serial.println();
  Serial.print("turn to right");
  analogWrite(leftMotorEN,255);
  analogWrite(rightMotorEN,255);
  digitalWrite(leftMotorForward,HIGH);
  digitalWrite(rightMotorForward,LOW);
  digitalWrite(rightMotorBackward,HIGH);
  digitalWrite(leftMotorBackward,LOW);
}

// STOP //
void Stop(void)   
{
  Serial.println();
  Serial.print("Stopped");
  analogWrite(leftMotorEN,0);
  analogWrite(rightMotorEN,0);
  digitalWrite(leftMotorForward,LOW);
  digitalWrite(leftMotorBackward,LOW);
  digitalWrite(rightMotorForward,LOW);
  digitalWrite(rightMotorBackward,LOW);
}


void setup() {

//********** CHANGE PIN FUNCTION  TO GPIO **********
//GPIO 1 (TX) swap the pin to a GPIO.
pinMode(1, FUNCTION_3); 
//GPIO 3 (RX) swap the pin to a GPIO.
pinMode(3, FUNCTION_3); 
//**************************************************


  /* motor control pins are here initialized as outputs */
  pinMode(leftMotorForward, OUTPUT);
  pinMode(rightMotorForward, OUTPUT); 
  pinMode(leftMotorBackward, OUTPUT);  
  pinMode(rightMotorBackward, OUTPUT);

  /* motor enable pins are initialized here as outputs */
  pinMode(leftMotorEN, OUTPUT); 
  pinMode(rightMotorEN, OUTPUT);

/* Ultrasonic sensor is initialized here: trigger as output and echo as input */
  pinMode(TRIGGERPIN, OUTPUT);
pinMode(ECHOPIN, INPUT);

/* Debug console is activated */
Serial.begin(115200); //debug console

/* Connection to Blynk server  is initialized */
// I did enable my local server connection, because I did work outside the home
Blynk.begin(auth, ssid, pass, server, 8080);

// I did disable Blynk cloud connection
//  Blynk.begin(auth, ssid, pass);
}


void loop() {
  /* run Blynk */

Blynk.run();

}